% Novy model gait.
% Zakladni myslenka je zalozena na jevu maximalizace 'security margin' pri vyuziti
% promenlive delky kroku a zvetsenim 'duty factor' na 83% z puvodne navrzenych 75%.
% The orignally selected 75% is the minimal duty factor for creeping gait. 

% Puvodni pohyb, X-table

% Xtable=[
%  9/9,   6/9,   3/9,   0/9,   1/9,   2/9,   3/9,   4/9,   5/9,   6/9,   7/9,   8/9;
%  3/9,   4/9,   5/9,   6/9,   7/9,   8/9,   9/9,   6/9,   3/9,   0/9,   1/9,   2/9;
%  6/9,   7/9,   8/9,   9/9,   6/9,   3/9,   0/9,   1/9,   2/9,   3/9,   4/9,   5/9;
%  0/9,   1/9,   2/9,   3/9,   4/9,   5/9,   6/9,   7/9,   8/9,   9/9,   6/9,   3/9];


% Zakladni popis pohybu quadpodu. Pohyb staticky vyvazeny, kruhovy. Security margin
% se navysil pomoci promenliveho kroku, duty factor porad ale 75%. 

% Xtable=[
% 	         +1        ||        +1/2       ||        +1         ||      +1/2
% 18/18, 14/18, 10/18,  6/18,  7/18,  8/18,  9/18, 11/18, 13/18, 15/18, 16/18, 17/18;
%  9/18, 11/18, 13/18, 15/18, 16/18, 17/18, 18/18, 14/18, 10/18,  6/18,  7/18,  8/18;
% 12/18, 14/18, 16/18, 18/18, 13/18,  8/18,  3/18,  5/18,  7/18,  9/18, 10/18, 11/18;
%  3/18,  5/18,  7/18,  9/18, 10/18, 11/18, 12/18, 14/18, 16/18, 18/18, 13/18,  8/18];

% Ztable=[
%   0  ,   1  ,   1  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0;
%   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   1  ,   1  ,   0  ,   0  ,   0;
%   0  ,   0  ,   0  ,   0  ,   1  ,   1  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0;
%   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   1  ,   1];


% Optimalizovany krok navysuje duty factor na 83%. Kombinaci navyseneho duty factor
% a promenlive delky kroku se zvysi staticka stabilita kroku. Tento gait je specielne
% navrzeny pro rotacni (zataceni) pohyb.
% Tento typ pohybu by mel jit pouzit i pro primocary pohyb. Nebude to asi moc rovnomerne,
% nerovnomernost se necha dohonit rozdilnym poctem kroku linearni regrese pouzite 
% pro jednotlive faze kroku.
% ale zajimalo by me, jak to muze fungovat. Je potreba ale pouzivat jinou titling
% table pro osu Z. V zakladu by mela stacit tabulka bez naklaneni, jak je uvedena nize.

Xtable=[
% 	         +1        ||        +1/2       ||        +1         ||      +1/2
18/18, 10/18,  4/18,  6/18,  7/18,  8/18,  9/18, 11/18, 13/18, 15/18, 16/18, 17/18;
 9/18, 11/18, 13/18, 15/18, 16/18, 17/18, 18/18, 10/18,  4/18,  6/18,  7/18,  8/18;
12/18, 14/18, 16/18, 18/18,  8/18,  2/18,  3/18,  5/18,  7/18,  9/18, 10/18, 11/18;
 3/18,  5/18,  7/18,  9/18, 10/18, 11/18, 12/18, 14/18, 16/18, 18/18,  1/18,  2/18];

Ztable=[
  0  ,  12  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0;
  0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,  12  ,   0  ,   0  ,   0  ,   0;
  0  ,   0  ,   0  ,   0  ,  12  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0;
  0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0  ,  12  ,   0];

% Ztable s 'smart tilting'. Robot se naklani aby ziskal co nejvetsi 'security margin' 
% pro staticky vyvazeny krok. Pro rotacni pohyb nei zatim implementovany,
% Ztable=[
% -12  ,   9  ,   9  ,   9  ,  12  ,  12  ,   0  ,   0  ,   0  ,   0  ,   0  ,   0;
%   0  ,   0  ,   0  ,   0  ,   0  ,   0  , -12  ,   9  ,   9  ,   9  ,  12  ,  12;
%   9  ,  12  ,  12  , -12  ,   9  ,   9  , -10  ,   0  ,   0  ,   0  ,   0  ,   0;
% -10  ,   0  ,   0  ,   0  ,   0  ,   0  ,   9  ,  12  ,  12  , -12  ,   9  ,   9];

printf("Xtable=[\n");
for arm=1:1:4
	for k=1:1:12
		y = d2fp( Xtable(arm,k));
		printf("0x%s, ", y);
	end
	printf(";\n");
end

printf("\n\n");

printf("Ztable=[\n");
for arm=1:1:4
	for k=1:1:12
		y = d2fp( Ztable(arm,k));
		printf("0x%s, ", y);
	end
	printf(";\n");
end

